/* SpanTreeC.nc
 * Authors : Mayank Mishra, Prabhakar Ekde,   CSE IIT-Bombay
 */
#include "printf.h"
#include <Timer.h>

#include "SpanTreeMsg.h"
#include "SpanTree.h"
 
module SpanTreeC {
    uses interface Boot;
    uses interface Leds;
    uses interface Timer<TMilli> as TimerSTHBBeacon;
    uses interface Timer<TMilli> as TimerSTHBBeaconRecv;
    uses interface Timer<TMilli> as TimerScanParent;
    uses interface Timer<TMilli> as TimerSeekParent;
    uses interface Timer<TMilli> as TimerRouteTableDisplay;

    uses interface AMSend as AMSendControlSync;
    uses interface AMSend as AMSendControlAsync;
    uses interface AMSend as AMSendData;
    
    uses interface SplitControl as AMControl;

    uses interface Receive as ReceiveControlSync;
    uses interface Receive as ReceiveControlAsync;
    uses interface Receive as ReceiveData;
    
    uses interface Random;
}

implementation {
    bool busyControlSync = FALSE;
    bool busyControlAsync = FALSE;
    bool busyData = FALSE;
    bool isSTBeaconTurn = TRUE;
    bool isTopoTurn = TRUE;
    
    message_t pktControlSync;
    message_t pktControlAsync;
    message_t pktData;
    
    uint8_t seekRetry = 0;
    
    /* Topology Control specification :
     * 1) Currently 20 motes are supported but it can easily be extended to more motes.
     * 2) The two Ids per element are the allowed parents which a mote will try to associate.
     * 3) Out of the two allowed parents specified the mote will select that parent which has 
     *    lower hop count to root.
     * 4) If both the allowed parent Ids are same then the mote has only one choice for parent.
     */
    bool topologyControlActive = TRUE;
    uint8_t topology[MAX_TOPOLOGY_ENTRIES][MAX_TOPOLOGY_CONTROL_OPTIONS] = {{0,0},
                                                                            {1,1},  /* Options for 1 */
                                                                            {1,4},  /*   ,,     ,, 2 */
                                                                            {2,2},  /*   ,,     ,, 3 */
                                                                            {3,3},  /*   ,,     ,, 4 */
                                                                            {3,3},  /*   ,,     ,, 5 */
                                                                            {4,4},  /*   ,,     ,, 6 */
                                                                            {5,5},  /*   ,,     ,, 7 */
                                                                            {7,7},  /*   ,,     ,, 8 */
                                                                            {8,8},  /*   ,,     ,, 9 */
                                                                            {9,9}}; /*   ,,     ,, 10 */
                             
     /* Following arrays specify the destination of the "RecRoute" and "TopoInfo" msgs
     * Current mote will select an entry from this array whose index is same as its Id.
     */
    uint8_t recRouteDest[MAX_TOPOLOGY_ENTRIES] = {0,1,1,1,5,3,7,1};
    uint8_t topoInfoDest[MAX_TOPOLOGY_ENTRIES] = {0,1,1,1,1,1,1,1};
    
    /* Only one of the following entries should be enabled */
    bool sendTopoInfo = TRUE;
    bool sendRecRoute = TRUE;

    /* This structure contains all the control information which a mote 
     * requires for functioning 
     */
    SpanTreeControl STControl;

    /* This routine sets the state of mote. It also sets proper LEDs to reflect this state. */
    void SpanTreeSetState(uint8_t state) {
        STControl.state = state;
        printf("\n Mote %d In State :", TOS_NODE_ID);
        switch (state) {
            case ORPHAN:
                printf("ORPHAN");
                call Leds.led0On();
                call Leds.led1Off();
                call Leds.led2Off();
                break;
            case SCANNING_PARENT:
                printf("SCANNING_PARENT");
                call Leds.led0On();
                call Leds.led1Off();
                call Leds.led2Off();
                break;
            case SEEKING_PARENT:
                printf("SEEKING_PARENT");
                call Leds.led0On();
                call Leds.led1Off();
                call Leds.led2Off();
                break;
            case JOINED_PARENT:
                printf("JOINED_PARENT");
                call Leds.led0Off();
                call Leds.led1On();
                call Leds.led2Off();
                break;
            case JOINED_PARENT_HAS_CHILDREN:
                printf("JOINED_PARENT_HAS_CHILDREN");
                call Leds.led0On();
                call Leds.led1On();
                call Leds.led2Off();
                break;
            default:
                printf("UNKNOWN");
                call Leds.led0On();
                call Leds.led1On();
                call Leds.led2On();
                break;
        }
        printfflush();
    }   

    event void Boot.booted() {
        call AMControl.start();
    }

    /* Potential Parent Routines */
    /* From the list of potential parents get the entry with least path vector length */
    SpanTreeParent* SpanTreeGetFirstPotentialParent(SpanTreePotentialParentList* parentList) {
        nx_uint8_t i;
        SpanTreeParent *firstParent = NULL;
    
        for (i = 0 ; i < MAX_NUM_POTENTIAL_PARENTS ; i++) {
            if (parentList->isValidPotentialParent[i] == TRUE) {
                if (firstParent == NULL) {
                    firstParent = &parentList->potentialParentEntry[i];
                 } else {
                     if (parentList->potentialParentEntry[i].pathVectorLength < 
                                                   firstParent-> pathVectorLength) {
                         firstParent = &(parentList->potentialParentEntry[i]);
                     }
                 }
            }
        }
        return firstParent;
    }

    /* From the list of potential parent get the entry which has next higher path vector length 
     * then the current entry under consideration
     */
    SpanTreeParent* SpanTreeGetNextPotentialParent(SpanTreeParent *currentPotentialParent, 
                                       SpanTreePotentialParentList* parentList) {
        nx_uint8_t i;
        SpanTreeParent *nextParent = NULL;
    
        for (i = 0 ; i < MAX_NUM_POTENTIAL_PARENTS ; i++) {
            if (parentList->isValidPotentialParent[i] == TRUE) {
                if ((currentPotentialParent->parentId != parentList->potentialParentEntry[i].parentId) &&
                     (parentList->potentialParentEntry[i].pathVectorLength >= currentPotentialParent->pathVectorLength)) {
                     if (nextParent == NULL) {
                          nextParent = &(parentList->potentialParentEntry[i]);
                     } else {
                          if (parentList->potentialParentEntry[i].pathVectorLength < 
                                                   nextParent-> pathVectorLength) {
                              nextParent = &(parentList->potentialParentEntry[i]);
                          }
                     }
                }
            }
        }
        return nextParent;
    }
    
    /* Add an entry in the potential parent list.. Following operations are done.
     * -- If the entry is already pesent then no need to add (We assume that in one scanning 
     *    period parent will be stable and maintain its path vector. In case parent's parent 
     *    has changed then the parent will go into scanning mode thus the time will be more 
     *    than the expiry time of the current nodes seek)
     * -- If entry is not present then we need to find an empty slot and put the entry there.
     * -- If there is no empty slot present then we need to replace the entry which has 
     *    highest path vector.
     */   
    void SpanTreeAddPotentialParent(uint8_t nodeId, uint16_t msgLen, SpanTreeSTBeaconMsg *msg) {
        uint8_t i, j;
        bool alreadyPresent = FALSE;
        uint8_t alreadyPresentIndex = 0;
        bool emptySlotFound = FALSE;
        uint8_t emptySlotIndex = 0;
        uint8_t highestVecLenIndex = -1;
        uint8_t tempPathVecLen = 0;
        
        /* Check if it is already in potential parent list */
        for (i = 0 ; i < MAX_NUM_POTENTIAL_PARENTS ; i ++) {
            if ((STControl.potentialParents.isValidPotentialParent[i]) &&
                (STControl.potentialParents.potentialParentEntry[i].parentId == nodeId)) {
                alreadyPresent = TRUE;
                alreadyPresentIndex = i;
                break;
            }
            if ((!emptySlotFound) && (STControl.potentialParents.isValidPotentialParent[i] == FALSE)) {
                emptySlotFound = TRUE;
                emptySlotIndex = i;
            }
            if (tempPathVecLen < STControl.potentialParents.potentialParentEntry[i].pathVectorLength) {
                highestVecLenIndex = i;
                tempPathVecLen = STControl.potentialParents.potentialParentEntry[i].pathVectorLength;
            }
        }
        
        /* Need to place the entry here in index if found = TRUE */
        if (alreadyPresent) {
             i = alreadyPresentIndex;
        } else if (emptySlotFound) {
             i = emptySlotIndex;
        } else {
             /* Replace the entry with highest pathvector */
             i = highestVecLenIndex;
        }
        STControl.potentialParents.potentialParentEntry[i].parentId = nodeId;
        STControl.potentialParents.isValidPotentialParent[i] = TRUE;
        STControl.potentialParents.potentialParentEntry[i].pathVectorLength = 
                                                             msg->pathVectorLength;
        for (j = 0 ; j < msg->pathVectorLength; j++) {
            STControl.potentialParents.potentialParentEntry[i].pathVector[j] = msg->list[j];
        }
    }

    /* Reset Routines 
     * Following Routines reset the different control elements
     */
    void SpanTreeResetPotentialParentList(SpanTreePotentialParentList* parentList) {
        nx_uint8_t i;
        for (i = 0 ; i < MAX_NUM_POTENTIAL_PARENTS ; i++) {
	     parentList->isValidPotentialParent[i] = FALSE;
        }
    }

    void SpanTreeResetChildrenList(SpanTreeChildrenList* children) {
        nx_uint8_t i;
        for (i = 0 ; i < MAX_NUM_CHILDREN ; i++) {
             children->isValidChild[i] = FALSE;
        }
    }

    void SpanTreeResetRouteTable(SpanTreeRouteTable* routeTable) {
        nx_uint8_t i;
        for (i = 0 ; i < ROUTE_TBL_MAX_ENTRIES ; i++) {
             routeTable->routeEntry[i].isValid = FALSE;
        }
    }

    void SpanTreeResetParent(SpanTreeParent* parent) {
        parent->parentId = INVALID_MOTE_ID;
        parent->pathVectorLength = INVALID_PATH_VECTOR_LEN;
    }

    void SpanTreeResetTimers(void) {
        call TimerSTHBBeacon.stop();
        call TimerSTHBBeaconRecv.stop();
        call TimerScanParent.stop();
        call TimerRouteTableDisplay.stop();      
    }

    /* This routines resets the complete mote. It sets the state of mote to ORPHAN */
    void SpanTreeResetMote(SpanTreeControl* moteControl) {
        if (DEBUG_STATE) {
            printf("\nMote %d Resetting Mote\n", TOS_NODE_ID);
            printfflush();
        }
        SpanTreeResetTimers();
        SpanTreeResetParent(&(moteControl->parent));
        STControl.numSTBeacFromParent = 0;
        STControl.state = ORPHAN;
        SpanTreeSetState(ORPHAN);
        SpanTreeResetChildrenList(&(moteControl->children));
        SpanTreeResetRouteTable(&(moteControl->routeTable));
        SpanTreeResetPotentialParentList(&(moteControl->potentialParents));
    }
    
    bool SpanTreeIsRootMote(void) {
        if (TOS_NODE_ID == ROOT_MOTE_ID)
            return TRUE;
        else
            return FALSE;
    }

    /* This routine Resets the mote and also sets the proper state along with setting proper timers */    
    void SpanTreeInitMote(void) {
        if (DEBUG_STATE) {
            printf("\nMote %d Initing\n", TOS_NODE_ID);
            printfflush();
        }
        SpanTreeResetMote (&STControl);
        if (TOS_NODE_ID == ROOT_MOTE_ID) {
            /* This is the Root mote -- Initialize properly */
            SpanTreeSetState(JOINED_PARENT);
            STControl.parent.parentId = ROOT_MOTE_ID;
            STControl.parent.pathVectorLength = INVALID_PATH_VECTOR_LEN;
            call TimerSTHBBeacon.startPeriodic(STHB_BEACON_PERIOD);
            call TimerSTHBBeaconRecv.startPeriodic(STHB_BEACON_TIMEOUT);
            call TimerRouteTableDisplay.startPeriodic(ROUTE_DISPLAY_PERIOD);
        } else {
            /* This is a non-Root mote -- need to scan to find out a parent */
            call TimerScanParent.startOneShot(PARENT_SCAN_PERIOD);
            SpanTreeSetState(SCANNING_PARENT);
        }
    }

    bool SpanTreeDropPkt(bool isControl) {
         uint8_t randomNum;
         randomNum = (uint8_t) (call Random.rand16());
         //printf("\nRandom num is %d ", randomNum);
         if (isControl) {
             if (PKT_DROP_PROBABILITY_CONTROL > randomNum) {
                 //printf("DROPPING ");
                 //printfflush();
                 return TRUE;
             }  
         } else {
             if (PKT_DROP_PROBABILITY_DATA > randomNum) {
                 //printf("DROPPING ");
                 //printfflush();
                 return TRUE;
             }
         }
         //printf("Not DROPPING ");
         //printfflush();
         return FALSE;
    }

    /* This is the Kick-Off routine */
    event void AMControl.startDone(error_t err) {
        if (err == SUCCESS) {
            SpanTreeInitMote();
        } else {
            call AMControl.start();
        }
    }

    /* The "busy" flag is used to signify that there is a transmission going on and next 
     * transmission should be avoided till "busy" is set to FALSE again
     */
    event void AMSendControlSync.sendDone(message_t* msg, error_t error) {
        if (&pktControlSync == msg) {
            busyControlSync = FALSE;
        }
    }

    event void AMSendControlAsync.sendDone(message_t* msg, error_t error) {
        if (&pktControlAsync == msg) {
            busyControlAsync = FALSE;
        }
    }

    event void AMSendData.sendDone(message_t* msg, error_t error) {
        if (&pktData == msg) {
            busyData = FALSE;
        }
    }
    
    /* This routines sets the correct parent 
     * It is called after we have done scanning and seeking
     */
    void SpanTreeSetParent(uint8_t parentId) {
        uint8_t i, j;
        for (i = 0 ; i < MAX_NUM_POTENTIAL_PARENTS ; i ++) {
            if ((STControl.potentialParents.isValidPotentialParent[i]) &&
                (STControl.potentialParents.potentialParentEntry[i].parentId == parentId)) {
                break;
            }
        }
        STControl.parent.parentId = parentId;
        STControl.parent.pathVectorLength = STControl.potentialParents.potentialParentEntry[i].pathVectorLength;
        for (j = 0 ; j < STControl.parent.pathVectorLength ; j++) {
            STControl.parent.pathVector[j] =  STControl.potentialParents.potentialParentEntry[i].pathVector[j];
        }
        /* To avoid parent rejection in next expiry timer check */
        STControl.numSTBeacFromParent = 1;
        if (DEBUG_CONTROL) {
            printf("\nMote %d Setting node %d as parent\n", TOS_NODE_ID, parentId);
            printfflush();
        }      
    }

    /* This routine returns TRUE if entry is already present or if an empty
     * slot was found and entry added otherwise it returns false.
     */
    bool SpanTreeAddChild(uint8_t nodeId) {
        uint8_t i;
        int8_t emptyIndex = -1;
        bool alreadyPresent = FALSE;
        for (i = 0 ; i < MAX_NUM_CHILDREN ; i ++) {
            if (STControl.children.isValidChild[i] == FALSE) {
                emptyIndex = i;
            } else {
                if (STControl.children.childEntry[i] == nodeId) {
                    alreadyPresent = TRUE;
                }
            }
        }
        if (alreadyPresent == FALSE) {
            if(emptyIndex == -1) {
                if (DEBUG_CONTROL) {
                    printf("\nMote %d Unable to add node %d as child\n", TOS_NODE_ID, nodeId);
                    printfflush();
                }
                return FALSE;
            }
            STControl.children.childEntry[emptyIndex] = nodeId;
            STControl.children.isValidChild[emptyIndex] = TRUE;
            /* Initializing the HB msg count to avoid child reject when
             * timer STHB_BEACON_TIMEOUT occurs
             */
            STControl.children.numHBMsg[emptyIndex] = 1;
            STControl.children.numChildren++;
        }
        if (DEBUG_CONTROL) {
            printf("\nMote %d Added node %d as child\n", TOS_NODE_ID, nodeId);
            printfflush();
        }
        return TRUE;
    }

    /* To remove the child */
    void SpanTreeRemoveChild(uint8_t nodeId) {
        uint8_t i;
        for (i = 0 ; i < MAX_NUM_CHILDREN ; i ++) {
            if ((STControl.children.isValidChild[i] == TRUE) && 
                                 (STControl.children.childEntry[i] == nodeId)) {
                    STControl.children.isValidChild[i] = FALSE;
                    STControl.children.numChildren--;
            }
        }
        if (STControl.children.numChildren == 0) {
             /* There are no children present so change state to JOINED_PARENT 
              * from JOINED_PARENT_HAS_CHILDREN 
              */
             SpanTreeSetState(JOINED_PARENT);
        }
        if (DEBUG_CONTROL) {
            printf("\nMote %d Removed node %d as child\n", TOS_NODE_ID, nodeId);
            printfflush();
        }
    }
    
    /* This routines utilises the path vector sent by a potential parent and checks 
     * whether there can arise a potential loop by joining it. The potential loop is 
     * detected by verifying whether the current mote appears in the ancestor list or
     * the path vector os the potential parent.
     */
    bool SpanTreeIsPathVectorLoopFree(SpanTreeSTBeaconMsg *msg) {
        uint8_t i;
        uint8_t moteId;
        bool loopFree = TRUE;     
        for (i = 0 ; i < msg->pathVectorLength ; i++) {
            moteId = msg->list[i];
            if (TOS_NODE_ID == moteId) {
                /* Potential routing loop -- Reject this beacon */
                loopFree = FALSE;
            }    
        }
        return loopFree;
    }

    /* In the children list, check whether the child is present */
    bool SpanTreeIsValidChild(uint8_t nodeId, uint8_t* location) {
        uint8_t i;
        for (i = 0 ; i < MAX_NUM_CHILDREN ; i ++) {
            if ((STControl.children.isValidChild[i] == TRUE) && 
                   (STControl.children.childEntry[i] == nodeId)) {
                    *location = i;
                    return TRUE;
            }
        }
        return FALSE;
    }    
    
    /* This routine utilises the Routing Table to figureout the next hop for a particular 
     * destination node. The routing table has all the entries for children and grandchildren.
     * For all other nodes "parent" is treated as the default route.
     */
    void SpanTreeGetNextHopNode(uint8_t nodeId, uint8_t *nextHop) {
        /* Simple logic - array index is the nodeId */        
        if ((nodeId < MAX_MOTE_ID) && (STControl.routeTable.routeEntry[nodeId].isValid == TRUE)) {
            *nextHop = STControl.routeTable.routeEntry[nodeId].nextHop;
            return;
        }
        *nextHop = STControl.parent.parentId;        
    
        /* logic 2 - Below logic optimises space but is more time consuming to find an entry */
		    /*  uint8_t i;
			bool found = FALSE;
			for (i = 0 ; i < ROUTE_TBL_MAX_ENTRIES ; i++) {
			    if (STControl.routeTable.routeEntry[i].isValid == TRUE) {
				if (STControl.routeTable.routeEntry[i].nodeId == nodeId) {
				    *nextHop = STControl.routeTable.routeEntry[i].nextHop;
				    found = TRUE;
				} 
			    }
			}
			if (found != TRUE) {
			    *nextHop = STControl.parent.parentId;
			}
		 */         
    }

    /* Following routine adds a route entry in the routing table */
    void SpanTreeAddToRouteTable(uint8_t nodeId, uint8_t nextHop) {
        /* Simple logic - array index is the nodeId */
        if (nodeId < MAX_MOTE_ID) {
            STControl.routeTable.routeEntry[nodeId].nextHop = nextHop;
            STControl.routeTable.routeEntry[nodeId].isValid = TRUE;
        }
     
        /* logic 2 - Below logic optimises space but is more time consuming to find an entry */
		     /* uint8_t i;
			int8_t emptyIndex = -1;
			bool found = FALSE;
			for (i = 0 ; i < ROUTE_TBL_MAX_ENTRIES ; i++) {
			    if (STControl.routeTable.routeEntry[i].isValid == TRUE) {
				if (STControl.routeTable.routeEntry[i].nodeId == nodeId) {
				    STControl.routeTable.routeEntry[i].nextHop = nextHop;
				    found = TRUE;
				} 
			    } else {
				emptyIndex = i;    
			    }
			}
			if ((found != TRUE) && (emptyIndex != -1)) {
			    STControl.routeTable.routeEntry[emptyIndex].nodeId = nodeId;
			    STControl.routeTable.routeEntry[emptyIndex].nextHop = nextHop;
			}
		 */
    }
    
    /* Following routine removes those enties from routing table whose next hop is the 
     * nodeId passed as parameter.
     */
    void SpanTreeRemoveFromRouteTable(uint8_t nodeId) {
        uint8_t i;
        for (i = 0 ; i < ROUTE_TBL_MAX_ENTRIES; i++) {
            /* ToDo : Need to think abt the following approach */
            if ((STControl.routeTable.routeEntry[i].nextHop == nodeId) || (i == nodeId)) {
                 STControl.routeTable.routeEntry[i].isValid = FALSE;
            }
        }
    }
        
    /* The HB (Heart Beat) message from child also lists all its children and 
     * grand children (in form of route entries). Thus the current mote's routing 
     * table is required to reflect this info. Following routine is used for it.
     */
    void SpanTreeUpdateRouteTable(uint8_t nextHopNode, SpanTreeHBMsg *msg) {
        uint8_t i;
        uint8_t nodeIdFromMsg;
      
        /* To Keep Routes Correct first flush the routes having next hop "nextHopNode" 
         * and enter afresh. This is done to ensure that the current mote's routing table
         * is not carrying any stale entry.
         * It is as good as a timer waking up and flushing but is more quicker.
         */
        SpanTreeRemoveFromRouteTable (nextHopNode);
        for (i = 0 ; i < (msg->numRoutes); i++) {
            nodeIdFromMsg = msg->routeList[i];
            SpanTreeAddToRouteTable (nodeIdFromMsg, nextHopNode);
        }
    }

    event void AMControl.stopDone(error_t err) {
    }

    /* Sending Asynchronus control messages ie which are not to be sent periodically */
    void SpanTreeSendControlAsyncMsg (uint16_t dstAddr, message_t* pkt, uint16_t msgLen, SpanTreeMsgType msgType) {
        bool busy = busyControlAsync;
        error_t err = FAIL;
        if (!busyControlAsync) {
            if ((err = (call AMSendControlAsync.send(dstAddr, pkt, msgLen))) == SUCCESS) {
                 busyControlAsync = TRUE;
            }
        } 
        if (!DEBUG_CONTROL) {
            return;
        }
        printf("\nMote %d Send Control Async Msg ", TOS_NODE_ID);
        switch(msgType) {
            case POISON_MSG:
                printf("POISON");
                break;
            case RELN_REQ:
                printf("RELN_REQ");
                break;
            case RELN_RESP_SUCCESS:
                printf("RELN_RESP_SUCC");
                break;
            case RELN_RESP_FAILURE:
                printf("RELN_RESP_FAIL");
                break;
            default :
                printf("UNKNOWN");
                break;
        }
        printf (" to %d",dstAddr);
        if (busy) {
            printf(" FAILED reason: Q-BUSY");
        } else {
            switch(err) {
                case SUCCESS:
                    printf("SUCCESS");
                    break;
                case FAIL:
                    printf("FAILED reason: AMSend FAIL");
                    break;
                case EBUSY:
                    printf("FAILED reason: AMSend EBUSY");
                    break;
                case ECANCEL:
                    printf("FAILED reason: AMSend ECANCEL");
                    break;
                default :
                    printf("FAILED reason: AMSend unknown err");
                    break;
             }
        }  
        printfflush();
    }

    /* Sending Synchronus control messages ie which are to be sent periodically */
    void SpanTreeSendControlSyncMsg (uint16_t dstAddr, message_t* pkt, uint16_t msgLen, SpanTreeMsgType msgType) {
        bool busy = busyControlSync;
        error_t err = FAIL;
        if (!busyControlSync) {
            if ((err = (call AMSendControlSync.send(dstAddr, pkt, msgLen))) == SUCCESS) {
                 busyControlSync = TRUE;
            }
        } 
        if (!DEBUG_CONTROL) {
            return;
        }
        printf("\nMote %d Send Control Sync Msg ", TOS_NODE_ID);
        switch(msgType) {
            case ST_BEACON:
                printf("ST_BEACON");
                break;
            case HB_MSG:
                printf("HB_MSG");
                break;
            default :
                printf("UNKNOWN");
                break;
        }
        printf (" to %d",dstAddr);
        if (busy) {
            printf(" FAILED reason: Q-BUSY");
        } else {
            switch(err) {
                case SUCCESS:
                    printf("SUCCESS");
                    break;
                case FAIL:
                    printf("FAILED reason: AMSend FAIL");
                    break;
                case EBUSY:
                    printf("FAILED reason: AMSend EBUSY");
                    break;
                case ECANCEL:
                    printf("FAILED reason: AMSend ECANCEL");
                    break;
                default :
                    printf("FAILED reason: AMSend unknown err");
                    break;
             }
        }  
        printfflush();
    }


    void SpanTreeSendDataMsg (uint16_t dstAddr, message_t* pkt, uint16_t msgLen, SpanTreeDataPayloadType msgType) {
        bool busy = busyData;
        error_t err = FAIL;
        if (!busyData) {
            if ((err = (call AMSendData.send(dstAddr, pkt, msgLen))) == SUCCESS) {
                 busyData = TRUE;
            }
        } 
        if (!DEBUG_DATA) {
            return;
        }
        printf("\nMote %d Send Data Msg ", TOS_NODE_ID);
        switch(msgType) {
            case DATA_INFO_PAYLOAD:
                printf("INFO_PAYLOAD");
                break;
            case DATA_REC_ROUTE_PAYLOAD:
                printf("REC_ROUTE_PAYLOAD");
                break;
            default :
                printf("UNKNOWN");
                break;
        }
        printf (" to %d",dstAddr);
        if (busy) {
            printf(" FAILED reason: Q-BUSY");
        } else {
            switch(err) {
                case SUCCESS:
                    printf("SUCCESS");
                    break;
                case FAIL:
                    printf("FAILED reason: AMSend FAIL");
                    break;
                case EBUSY:
                    printf("FAILED reason: AMSend EBUSY");
                    break;
                case ECANCEL:
                    printf("FAILED reason: AMSend ECANCEL");
                    break;
                default :
                    printf("FAILED reason: AMSend unknown err");
                    break;
             }
        }
        printfflush();
    }

    /* Following routine generates the ST (Spanning Tree) Beacon 
     * The ST Beacon contains following info
     * 1) The path vector
     * 2) The children list : This is kept to ensure that the child receiving it 
     *   knows that it is still treated as child by the current mote or not. If the
     *   child doesnot finds its own entry in this list then it will asume that parent
     *   has rejected it and then child will take necessary actions.
     */ 
    void SpanTreeGenerateSTBeacon(SpanTreeGenericMsg* stBeaconPkt) {
        SpanTreeSTBeaconMsg* stBeacon;
        nx_uint8_t* childrenList;
        uint8_t i, j,numChildren = 0;
        stBeaconPkt->nodeId = TOS_NODE_ID;
        stBeaconPkt->msgType = ST_BEACON;
        stBeacon = (SpanTreeSTBeaconMsg*) (stBeaconPkt->msgPayload);
        stBeacon->pathVectorLength = STControl.parent.pathVectorLength + 1;
        
        for (i = 0, j = 0; i < STControl.parent.pathVectorLength; i++, j++) {
            stBeacon->list[j] = STControl.parent.pathVector[i];
        }
        /* Need to add own's entry in the path vector */
        stBeacon->list[j++] = TOS_NODE_ID;
        
        for (i = 0 ; i < MAX_NUM_CHILDREN ; i++) {
            if (STControl.children.isValidChild[i]) {
                stBeacon->list[j++] = STControl.children.childEntry[i];
                childrenList++;
                numChildren++;
            }
        }
        stBeacon->numChildren = numChildren;
        stBeaconPkt->msgLen = stBeacon->pathVectorLength + stBeacon->numChildren + ST_BEACON_MSG_OVERHEAD;
    }

    /* Following routine generates and sends the ST beacon */
    void SpanTreeGenerateAndSendSTBeacon (void) {
        SpanTreeGenericMsg* stSTBeacon = (SpanTreeGenericMsg*)(call AMSendControlSync.getPayload(&pktControlSync, NULL));
        if (STControl.state == ORPHAN) {
            return;
        }
        SpanTreeGenerateSTBeacon (stSTBeacon);
        SpanTreeSendControlSyncMsg (AM_BROADCAST_ADDR, &pktControlSync, stSTBeacon->msgLen, ST_BEACON);
    } 

    /* Following routine generates the HB message. It contains following
     * 1) The routing table of the current mote to be given to parent 
     */
    void SpanTreeGenerateHBMsg (SpanTreeGenericMsg* stHBPkt) {
        SpanTreeHBMsg* hbMsg;
        uint8_t i, numRoutes = 0;
        stHBPkt->nodeId = TOS_NODE_ID;
        stHBPkt->msgType = HB_MSG;
        hbMsg = (SpanTreeHBMsg*) (stHBPkt->msgPayload);
        for (i = 0 ; i < ROUTE_TBL_MAX_ENTRIES ; i++) {
            if (STControl.routeTable.routeEntry[i].isValid)  {
                hbMsg->routeList[numRoutes] = i;
                numRoutes++;
            }
        }
        hbMsg->numRoutes = numRoutes;
        stHBPkt->msgLen = hbMsg->numRoutes + HB_MSG_OVERHEAD;
    }
    
    /* The argument is void here because HB msg can only be sent to Parent */
    void SpanTreeGenerateAndSendHBMsg(void) {
        SpanTreeGenericMsg* stHBMsg = (SpanTreeGenericMsg*)(call   AMSendControlSync.getPayload(&pktControlSync, NULL));
        SpanTreeGenerateHBMsg (stHBMsg);
        SpanTreeSendControlSyncMsg (STControl.parent.parentId, &pktControlSync, stHBMsg->msgLen, HB_MSG);
    }

    void SpanTreeGeneratePoisonMsg(SpanTreeGenericMsg* poisonPkt) {
        poisonPkt->nodeId = TOS_NODE_ID;
        poisonPkt->msgType = POISON_MSG;
        poisonPkt->msgLen = 0;
    }

    void SpanTreeGenerateAndSendPoisonMsg (uint16_t dstAddr) {
        /* Dst address can be broadcast address */
        SpanTreeGenericMsg* stPoisonPkt = (SpanTreeGenericMsg*)(call AMSendControlAsync.getPayload(&pktControlAsync, NULL));
        SpanTreeGeneratePoisonMsg (stPoisonPkt);
        SpanTreeSendControlAsyncMsg (dstAddr, &pktControlAsync, stPoisonPkt->msgLen + ST_GENERIC_MSG_OVERHEAD, POISON_MSG);
    }

    void SpanTreeGenerateRelationReqMsg(SpanTreeGenericMsg* relnReqPkt) {
        relnReqPkt->nodeId = TOS_NODE_ID;
        relnReqPkt->msgType = RELN_REQ;
        relnReqPkt->msgLen = 0;
    }

    void SpanTreeGenerateAndSendRelationReqMsg(uint8_t dstAddr) {
        SpanTreeGenericMsg* stRelnReqPkt = (SpanTreeGenericMsg*)(call   AMSendControlAsync.getPayload(&pktControlAsync, NULL));
        SpanTreeGenerateRelationReqMsg (stRelnReqPkt);
        SpanTreeSendControlAsyncMsg (dstAddr, &pktControlAsync, stRelnReqPkt->msgLen + ST_GENERIC_MSG_OVERHEAD, RELN_REQ);
    }

    void SpanTreeGenerateRelationRespMsg(SpanTreeGenericMsg* relnRespPkt, bool isSuccess) {
        relnRespPkt->nodeId = TOS_NODE_ID;
        if (isSuccess)
            relnRespPkt->msgType = RELN_RESP_SUCCESS;
        else
            relnRespPkt->msgType = RELN_RESP_FAILURE;
        relnRespPkt->msgLen = 0;
    }

    void SpanTreeGenerateAndSendRelationRespMsg(uint8_t dstAddr, bool isSuccess) {
        SpanTreeGenericMsg* stRelnRespPkt = (SpanTreeGenericMsg*)(call AMSendControlAsync.getPayload(&pktControlAsync, NULL));
        SpanTreeGenerateRelationRespMsg (stRelnRespPkt, isSuccess);
        SpanTreeSendControlAsyncMsg (dstAddr, &pktControlAsync, stRelnRespPkt->msgLen + ST_GENERIC_MSG_OVERHEAD, 
                                      isSuccess ? RELN_RESP_SUCCESS : RELN_RESP_FAILURE);
    }

    /* Following routines seeks potential parents. It does the following.
     * 1) chooses the potential parent with lowest path vector first and seeks it for a relation.
     * 2) do 3 retries are done each after a delay of 100ms.
     * 3) if a parent doesnot accepts even after 3 retries then choose next lowest parent
     * 4) Go To 2)
     */  
    void SpanTreeSeekParents(bool calledByTimer) {
        SpanTreeParent* potentialParent = NULL;
        bool firstSeekParent = FALSE;
        
        if(STControl.state != SEEKING_PARENT) {
            return;
        }
        
        if (!calledByTimer) {
            /* To ensure that first potential parent is tried */
            seekRetry = MAX_SEEK_RETRIES + 1;
            firstSeekParent = TRUE;
        }
        
        if (seekRetry > MAX_SEEK_RETRIES) {
            if (firstSeekParent) {
                potentialParent = SpanTreeGetFirstPotentialParent(&(STControl.potentialParents));
                firstSeekParent = FALSE;
            } else {
                potentialParent = SpanTreeGetNextPotentialParent(potentialParent, &(STControl.potentialParents));
            }
            if (potentialParent == NULL) {
                call TimerSeekParent.stop();
                call TimerScanParent.startOneShot(PARENT_SCAN_PERIOD);
                return;
            }
            seekRetry = 1;
        }
        if (DEBUG_CONTROL) {
            printf("\n Mote %d SEEKING node %d retry %d\n", TOS_NODE_ID, potentialParent->parentId, seekRetry);
            printfflush();
        }
        SpanTreeGenerateAndSendRelationReqMsg (potentialParent->parentId);
        seekRetry++;
        call TimerSeekParent.startOneShot(SEEK_RETRY_PERIOD);
    }

    /* Following routine is the Forwarding logic */
    void SpanTreeForwardDataMsg(uint8_t nextHop, SpanTreeGenericMsg *msg) {
        SpanTreeDataMsg* dataMsg;
        SpanTreeRecRoutePayload* recRoutePayload;
        SpanTreeGenericMsg* stGeneric = (SpanTreeGenericMsg*)(call   AMSendData.getPayload(&pktData, NULL));
        memcpy(stGeneric, msg, msg->msgLen + ST_GENERIC_MSG_OVERHEAD);
        dataMsg = (SpanTreeDataMsg*) stGeneric->msgPayload;
        if (dataMsg->payloadType == DATA_REC_ROUTE_PAYLOAD) {
            /* We need to record out entry in this message */
            recRoutePayload = (SpanTreeRecRoutePayload*)dataMsg->payload;
            recRoutePayload->routeHops[recRoutePayload->numRouteHops] = TOS_NODE_ID;
            recRoutePayload->numRouteHops++;
            stGeneric->msgLen++;
        }
        if (DEBUG_ROUTING) {
            printf("\nMote %d Forwarding Msg to %d\n", TOS_NODE_ID, nextHop);
            printfflush();
        }
        SpanTreeSendDataMsg (nextHop, &pktData, stGeneric->msgLen + ST_GENERIC_MSG_OVERHEAD, DATA_REC_ROUTE_PAYLOAD);
    }

    void SpanTreeDisplayTopoInfo(SpanTreeGenericMsg* stGeneric) {
        uint8_t i, j;
        SpanTreeDataMsg* dataMsg;
        SpanTreeInfoPayload* infoPayload;
        uint8_t numMem;
        if (!DEBUG_INFO) {
            return;
        }
        dataMsg = (SpanTreeDataMsg*)stGeneric->msgPayload;
        infoPayload = (SpanTreeInfoPayload *)dataMsg->payload;
        
        printf("\n Topo-info of %d Parent %d Children(num %d) ", stGeneric->nodeId, 
                infoPayload->parentId, infoPayload->numChildren);
        numMem = infoPayload->numChildren;
        for (i = 0 ; i < numMem; i++) {
            printf("%d, ", infoPayload->list[i]);
        }
        printf(" Routes(num %d) ", infoPayload->numRouteEntries);
        numMem += (2 * (infoPayload->numRouteEntries));
        for (; i < numMem; i = i + 2) {
            printf("%d nh %d, ", infoPayload->list[i], infoPayload->list[i + 1]);
        }
        printf("Def rt %d", infoPayload->parentId);
        printfflush();
    }
    
    void SpanTreeDisplayRecordedRoute(SpanTreeGenericMsg* stGeneric) {
        uint8_t i;
        SpanTreeDataMsg* dataMsg;
        SpanTreeRecRoutePayload* recRoutePayload;
        uint8_t numMem;
        if (!DEBUG_INFO) {
            return;
        }
        dataMsg = (SpanTreeDataMsg*)stGeneric->msgPayload;
        recRoutePayload = (SpanTreeRecRoutePayload *)dataMsg->payload;
        
        printf("\nMote %d RecRoute payload rcvd from %d", TOS_NODE_ID, stGeneric->nodeId);
        printf("\t Route taken : %d ", stGeneric->nodeId);
        numMem = recRoutePayload->numRouteHops;
        for (i = 0 ; i < numMem; i++) {
            printf("-> %d ", recRoutePayload->routeHops[i]);
        }
        printf("-> %d",TOS_NODE_ID);
        printfflush();
    }
    
    void SpanTreeGenerateRecRouteMsg(SpanTreeGenericMsg* stGeneric) {
        uint8_t numRoutes = 0;
        SpanTreeDataMsg* dataMsg;
        SpanTreeRecRoutePayload* recRoutePayload; 
        stGeneric->nodeId = TOS_NODE_ID;
        stGeneric->msgType = DATA_MSG;
        dataMsg = (SpanTreeDataMsg*) (stGeneric->msgPayload);
        
        dataMsg->destId = recRouteDest[TOS_NODE_ID];
        dataMsg->payloadType = DATA_REC_ROUTE_PAYLOAD;
        recRoutePayload = (SpanTreeRecRoutePayload *)dataMsg->payload;
        recRoutePayload->numRouteHops = 0;
        stGeneric->msgLen = REC_ROUTE_DATA_MSG_OVERHEAD;
    }

    void SpanTreeGenerateAndSendRecRouteMsg(void) {
        uint8_t nextHop;
        SpanTreeGenericMsg* stGeneric = (SpanTreeGenericMsg*)(call   AMSendData.getPayload(&pktData, MAX_PKT_SIZE));
        if (TOS_NODE_ID == ROOT_MOTE_ID) {
            return;
        }
        SpanTreeGenerateRecRouteMsg(stGeneric);
        SpanTreeGetNextHopNode(recRouteDest[TOS_NODE_ID], &nextHop);
        SpanTreeSendDataMsg (nextHop, &pktData, stGeneric->msgLen, DATA_REC_ROUTE_PAYLOAD);
    }

    void SpanTreeGenerateTopoInfoMsg(SpanTreeGenericMsg* stGeneric) {
        uint8_t i, j;
        uint8_t numRoutes = 0;
        SpanTreeDataMsg* dataMsg;
        SpanTreeInfoPayload* infoPayload; 
        stGeneric->nodeId = TOS_NODE_ID;
        stGeneric->msgType = DATA_MSG;
        dataMsg = (SpanTreeDataMsg*) (stGeneric->msgPayload);
        /* Currently this message is to be sent till Root */
        dataMsg->destId = topoInfoDest[TOS_NODE_ID];
        dataMsg->payloadType = DATA_INFO_PAYLOAD;
        infoPayload = (SpanTreeInfoPayload *)dataMsg->payload;
        infoPayload->parentId = STControl.parent.parentId;
        for (i = 0, j = 0; i < MAX_NUM_CHILDREN; i++) {
            if (STControl.children.isValidChild[i])  {
                infoPayload->list[j++] = STControl.children.childEntry[i];
            }
        }
        infoPayload->numChildren = j;
        for (i = 0 ; i < ROUTE_TBL_MAX_ENTRIES ; i++) {
            if (STControl.routeTable.routeEntry[i].isValid)  {
                infoPayload->list[j++] = i; /* Node Id */
                infoPayload->list[j++] = STControl.routeTable.routeEntry[i].nextHop;
                numRoutes++;
            }
        }
        infoPayload->numRouteEntries = numRoutes;
        stGeneric->msgLen = TOPO_INFO_DATA_MSG_OVERHEAD + infoPayload->numChildren + (2*numRoutes);
    }
    
    void SpanTreeGenerateAndSendTopoInfoMsg(void) {
        uint8_t nextHop;
        SpanTreeGenericMsg* stGeneric = (SpanTreeGenericMsg*)(call   AMSendData.getPayload(&pktData, MAX_PKT_SIZE));
        SpanTreeGenerateTopoInfoMsg(stGeneric);
        if (DEBUG_INFO) {
            SpanTreeDisplayTopoInfo(stGeneric);
        }
        if (TOS_NODE_ID != ROOT_MOTE_ID) {
            SpanTreeGetNextHopNode(topoInfoDest[TOS_NODE_ID], &nextHop);
            SpanTreeSendDataMsg (nextHop, &pktData, stGeneric->msgLen, DATA_INFO_PAYLOAD);
        }   
    }


    event void TimerSeekParent.fired() {
        SpanTreeSeekParents(TRUE);
    }

    event void TimerRouteTableDisplay.fired() {
        /* Generate and send Topo info to Root */
        if (!DEBUG_INFO) {
            return;
        }
        if (isTopoTurn && sendTopoInfo) {
            SpanTreeGenerateAndSendTopoInfoMsg();
        }
        
        if (!isTopoTurn && sendRecRoute) {
            SpanTreeGenerateAndSendRecRouteMsg();
        }
        isTopoTurn = !isTopoTurn;
    }

    event void TimerScanParent.fired() {
        /* Try to setup relation with potential parent in order of increasing path vector length */
        uint8_t i, j, numPotParents = 0;
        if (STControl.state == JOINED_PARENT || STControl.state == JOINED_PARENT_HAS_CHILDREN) {
            return;
        }
        for (i = 0 ; i < MAX_NUM_POTENTIAL_PARENTS ; i++) {
             if (STControl.potentialParents.isValidPotentialParent[i]) {
                 numPotParents++;
             }
        }
        if (0 == numPotParents) {
             /* Start Scanning Again */
            call TimerScanParent.startOneShot(PARENT_SCAN_PERIOD);
        } else {
            /* Start Seeking Potential Parents */
            SpanTreeSetState(SEEKING_PARENT); 
            SpanTreeSeekParents(FALSE);
        }
    }
    
    event void TimerSTHBBeaconRecv.fired() {
        uint8_t i;

        /* Need to check if 0 ST msgs received then 
         * - need to send poison messages to every child (We can broadcast) 
         * - need to reset mote and start scanning for parent again
         * NOTE - This needs to be done only for non-Root mote
         */
        
        if (!SpanTreeIsRootMote()) {
            if (0 == STControl.numSTBeacFromParent) {           
                /* ST Beacon timeout has occured. Poison all children and Re-Init Mote */
                if (DEBUG_CONTROL) {
                    printf("\nMote %d Not Rcvd ST beacon from parent %d in last %dms re-initing\n", TOS_NODE_ID, 
                        STControl.parent.parentId, STHB_BEACON_TIMEOUT);
                    printfflush();
                }
                SpanTreeGenerateAndSendPoisonMsg (AM_BROADCAST_ADDR);
                SpanTreeInitMote();
            } else {
                STControl.numSTBeacFromParent = 0;
            }
        }

        /* Need to check if 0 HB msgs received from any child, say C, then 
         * - need to send poison messages to C
         * - remove the child entry C from children list
         * - remove all route entries whose next-hop is C  
         */
         /* Possible optimisations can be to do following process when the state is 
          * JOINED_PARENT_HAS_CHILDREN... but leaving it for now and following 
          * principle of KISS
          */
        for (i = 0 ; i < MAX_NUM_CHILDREN ; i++) {
            if (TRUE == STControl.children.isValidChild[i]) {
                if (0 == STControl.children.numHBMsg[i]) {
                    /* Initiate the child reject ie Poison the child*/
                    SpanTreeGenerateAndSendPoisonMsg (STControl.children.childEntry[i]);
                    /* Remove the corresponding route entries */
                    SpanTreeRemoveFromRouteTable(STControl.children.childEntry[i]);
                    /* Remove the child from children list */
                    SpanTreeRemoveChild(STControl.children.childEntry[i]);
                } else {
                    /* Child is still alive - re-init the counter */
                    STControl.children.numHBMsg[i] = 0;
                }
            }
        }
    }
    
    /* This timer fires every STHB_BEACON_PERIOD time and broadcasts a ST Beacon and
     * unicasts a HB message to Parent alterntely.
     */
    event void TimerSTHBBeacon.fired() {
        if ((STControl.state == JOINED_PARENT) || (STControl.state == JOINED_PARENT_HAS_CHILDREN)) {
            if (isSTBeaconTurn) {
                SpanTreeGenerateAndSendSTBeacon ();
                if (!SpanTreeIsRootMote()) {
                    isSTBeaconTurn = FALSE;
                }
            } else {
                /* If mote is root then we dont need to send any HB to parent.. quiz-- why?? */
                if (!SpanTreeIsRootMote()) {
                    SpanTreeGenerateAndSendHBMsg ();
                    isSTBeaconTurn = TRUE;
                }
            }
        }
    }
    
    void SpanTreeHandleHBMsg(uint8_t nodeId, uint16_t msgLen, SpanTreeHBMsg *msg) {
        uint8_t childIndex;
        if (STControl.state != JOINED_PARENT_HAS_CHILDREN) {
            /* Ignore This message as some useless fellow is trying to behave like child or this parent is careless
             * Optionally a Poison message can be sent ;) 
             */
             if (DEBUG_ERROR) {
                 printf("\nMote %d Rcvd HB msg from a non-child %d\n", TOS_NODE_ID, nodeId);
                 printfflush();
             }
        } else {
            /* Is the Beacon from valid child */
                 /* If yes then accept the Msg and update routing table */
                 /* If not then send poison message */
            if (SpanTreeIsValidChild(nodeId, &childIndex)){
                STControl.children.numHBMsg[childIndex]++;
                /* Update Routing Table */
                SpanTreeUpdateRouteTable (nodeId, msg);
                /* Update 1 hop child route */
                SpanTreeAddToRouteTable (nodeId, nodeId);
            } 
        }
    }

    void SpanTreeHandlePoisonMsg(uint8_t nodeId) {
        /* Poison should not be taken from anyone.. Only parents have the right to poison :) (scary situation)*/
        if (STControl.parent.parentId != nodeId) {
            return;
        }
        if (DEBUG_CONTROL) {
            printf("\nMote %d Rcvd poison msg from parent initing\n", TOS_NODE_ID);
            printfflush();
        }
        if (STControl.state == JOINED_PARENT) {
            SpanTreeInitMote();
        } else if (STControl.state == JOINED_PARENT_HAS_CHILDREN){
            /* Poison all the children */
            SpanTreeGenerateAndSendPoisonMsg(AM_BROADCAST_ADDR);
            /* Re-init the mote */
            SpanTreeInitMote();
        }
    }

    void SpanTreeHandleRelationReqMsg(uint8_t nodeId) {
        if ((STControl.state == JOINED_PARENT) || (STControl.state == JOINED_PARENT_HAS_CHILDREN)) {
            /* By default accept the request */
            if (DEBUG_CONTROL) {
                printf("\nMote %d rcvd Reln Req msg from %d\n", TOS_NODE_ID, nodeId);
                printfflush();
            }
            if (TRUE == SpanTreeAddChild(nodeId)) {
                SpanTreeAddToRouteTable (nodeId, nodeId);
                SpanTreeSetState (JOINED_PARENT_HAS_CHILDREN);
                SpanTreeGenerateAndSendRelationRespMsg(nodeId, TRUE);
            }
        }
    }

    void SpanTreeHandleRelationRespSuccessMsg(uint8_t nodeId) {
        /* This message should be accepted only when we are in SEEK state */
        if (STControl.state == SEEKING_PARENT) {
            /* We got an acceptance from parent */
            /* Do following things 
             * 1) Change the state to JOINED_PARENT
             * 2) Update the control structure
             * 3) Start the periodic ST Beacon timer and HB timer
             * 4) Stop the Scanning timer (must be already stopped already)
             */
            if (DEBUG_CONTROL) {
                printf("\nMote %d rcvd Reln Resp success msg from %d\n", TOS_NODE_ID, nodeId);
                printfflush();
            }
            SpanTreeSetState (JOINED_PARENT);
            SpanTreeSetParent(nodeId);
            call TimerSTHBBeacon.startPeriodic(STHB_BEACON_PERIOD);
            call TimerSTHBBeaconRecv.startPeriodic(STHB_BEACON_TIMEOUT);
            call TimerRouteTableDisplay.startPeriodic(ROUTE_DISPLAY_PERIOD);
        } 
    }

    void SpanTreeHandleRelationRespFailMsg(uint8_t nodeId) {
         /* Currently this situation can not happen */
    }

    bool SpanTreeIsMotePresentInChildrenList(SpanTreeSTBeaconMsg *msg) {
        uint8_t i;
        uint8_t moteId;
        for (i = 0 ; i < msg->numChildren; i++) {
            moteId = msg->list[i+msg->pathVectorLength];
            if (TOS_NODE_ID == moteId) {
                /* I am still the child */
                return TRUE;
            }
        }
        return FALSE;
    }
    
    /* This routines checks whether topology control is active or not. If topology control is active it checks
     * whether the node from which beacon is received is an allowed parent or not.
     */
    bool SpanTreeIsAllowedParent(uint8_t nodeId) {
        int i;
        if (topologyControlActive) {
            for (i = 0 ; i < MAX_TOPOLOGY_CONTROL_OPTIONS; i++) {
                if (nodeId == topology[TOS_NODE_ID][i]) {
                    return TRUE;
                }
            }
            return FALSE;
        }
        return TRUE;
    }

    /* This function will be triggered when a ST beacon is received */
    void SpanTreeHandleSTBeacon(uint8_t nodeId, uint16_t msgLen, SpanTreeSTBeaconMsg *msg) {
        /* Root doesn't require to listen to ST beacons */
        
        if(SpanTreeIsRootMote()) {
            return;
        }
        
        /* Topology Control */
        if (!SpanTreeIsAllowedParent(nodeId)) {
            return;
        }
        /* Mote can accept ST beacons under all situations except when it is seeking */
        if (STControl.state == SEEKING_PARENT) {
            return;
        }
        /* To avoid loops we need to check whether the current node is in the received PathVector
         * If it is present then it means that the beacon is coming from a mote which is
         * current mote's child. This should be avoided.
         */
        if (FALSE == SpanTreeIsPathVectorLoopFree(msg)) {
            return;
        }
        if (DEBUG_CONTROL) {
            printf("\nMote %d Rcvd ST_BEACON from %d", TOS_NODE_ID, nodeId);
            printfflush();
        }
        /* Path Vector is loop free we can continue */
        if (STControl.state == SCANNING_PARENT) {
            /* Put this message's sender in potential parent list */
            SpanTreeAddPotentialParent(nodeId, msgLen, msg);
        } else {
            /* Is the Beacon from parent */
            if (nodeId == STControl.parent.parentId) {
                STControl.numSTBeacFromParent++;
                /* See whether the current node is present in the children list 
                 * received in ST Beacon from parent.
                 */
                if(!SpanTreeIsMotePresentInChildrenList(msg)) {
                    /* Mote got disowned -- need to 
                     * - Send Poison MSG to all Children
                     * - Change self state to ORPHAN
                     * - Start scanning for parent
                     */
                     if (DEBUG_CONTROL) {
                         printf("\nMote %d got Disowned by Parent\n");
                         printfflush();
                     }
                     SpanTreeGenerateAndSendPoisonMsg(AM_BROADCAST_ADDR);
                     SpanTreeInitMote();
                }
            }
        }
    } 
        
    void SpanTreeHandleDataMsg(SpanTreeGenericMsg *msg) {
        SpanTreeDataMsg *dataMsg;
        uint8_t nextHop;
        dataMsg = (SpanTreeDataMsg *)msg->msgPayload;
        /* Check Whether this mote is the destination */
        if (TOS_NODE_ID == dataMsg->destId) {
            /* This message is for us we need to display it through printf */
            if (dataMsg->payloadType == DATA_INFO_PAYLOAD) { 
                SpanTreeDisplayTopoInfo(msg);
            } else {
                SpanTreeDisplayRecordedRoute(msg);
            }
        } else {
            /* This message is not for us, lets forward it to its desired node */
            SpanTreeGetNextHopNode(dataMsg->destId, &nextHop);
            SpanTreeForwardDataMsg(nextHop, msg);
            if (DEBUG_DATA) {
                printf("\nMote %d Rcvd %s Msg destined for %d forwarded to %d\n", TOS_NODE_ID, 
                       DATA_INFO_PAYLOAD?"Topo Info":"Rec Route" ,dataMsg->destId, nextHop);
                printfflush();
            }
        }
    }
    
    event message_t* ReceiveControlSync.receive(message_t* msg, void* payload, uint8_t len) {
        SpanTreeGenericMsg* stMessage = (SpanTreeGenericMsg*)payload;
        /* Check msg type */
        if (!SpanTreeDropPkt (TRUE)) {
            switch(stMessage->msgType) {
                case ST_BEACON :
                    SpanTreeHandleSTBeacon(stMessage->nodeId, stMessage->msgLen,
                                              (SpanTreeSTBeaconMsg*)&(stMessage->msgPayload));
                    break;
                case HB_MSG :
                    SpanTreeHandleHBMsg(stMessage->nodeId, stMessage->msgLen, 
                                           (SpanTreeHBMsg*)&(stMessage->msgPayload));
                    break;
                default:
                    break;
            }       
        }
        return msg;
    }    

    event message_t* ReceiveControlAsync.receive(message_t* msg, void* payload, uint8_t len) {
        SpanTreeGenericMsg* stMessage = (SpanTreeGenericMsg*)payload;
        /* Check msg type */
        if (!SpanTreeDropPkt (TRUE)) {
            switch(stMessage->msgType) {
                case POISON_MSG :
                    SpanTreeHandlePoisonMsg(stMessage->nodeId);
                    break;
                case RELN_REQ:
                    SpanTreeHandleRelationReqMsg(stMessage->nodeId);
                    break;
                case RELN_RESP_SUCCESS:
                    SpanTreeHandleRelationRespSuccessMsg(stMessage->nodeId);
                    break;
                case RELN_RESP_FAILURE:
                    SpanTreeHandleRelationRespFailMsg(stMessage->nodeId);
                    break;
                default:
                    break;
            }
        }       
        return msg;
    }    


    event message_t* ReceiveData.receive(message_t* msg, void* payload, uint8_t len) {
        SpanTreeGenericMsg* stMessage = (SpanTreeGenericMsg*)payload;
        /* Check msg type */
        if (!SpanTreeDropPkt (FALSE)) {
            switch(stMessage->msgType) {
                case DATA_MSG:
                    SpanTreeHandleDataMsg(stMessage);
                    break;
                default:
                    break;
            }
        }       
        return msg;
    }    
}

